#include "defines.h"
#include "rpe_ltp_tx.h"

// According to the standard this is the expected format of the input samples :
// S.v.v.v.v.v.v.v.v.v.v.v.v.x.x.x ( 2's complement format).
// Where S is the sign bit, v a valid bit, and x a "don't care" bit.
// The original signal is called sop[..];

// Downscaling of the input signal(5.2.1)
void input_downscale(int16_t* input) {
	int i;
	for(i=0;i<FRAME_LENGTH;i++) {// TODO : use defines for constants(low prio)
		input[i]>>=3;
		input[i]<<=2;
	}
}

// Offset compensation(5.2.2)
// sof[k]=so[k]-so[k-1]+alpha*sof[k-1]
// alpha=32735*2^-5
void offset_compensation(int16_t* so,int16_t* sof,int16_t* z1,int32_t* L_z2) {
	int k;
	int16_t s1,msp,lsp,temp;
	int32_t L_s2;

	for(k=0;k<FRAME_LENGTH;k++) {
		// Compute the non-recursive part
		s1=fxp_sub(so[k],*z1);
		*z1=so[k];
		// Compute the recursive part
		L_s2=s1;
		L_s2=L_s2<<15;
		// 31 by 16 bits multiplication
		msp=*L_z2>>15;
		lsp=(int16_t)fxp_L_sub(*L_z2,(int32_t)(msp<<15));
		temp=fxp_mult_r(lsp,32375);
		L_s2=fxp_L_add(L_s2,temp);
		*L_z2=fxp_L_add(fxp_L_mult(msp,32375)>>1,L_s2);
		// Compute sof[k] with rounding
		sof[k]=(int16_t)(fxp_L_add(*L_z2,16384)>>15);
	}
}

// Pre-emphasis(5.2.3)
void pre_emphasis(int16_t* sof,int16_t* s,int16_t* mp) { // todo correct pointer references
	int k;
	for(k=0;k<FRAME_LENGTH;k++) {
		s[k]=fxp_add(sof[k],fxp_mult_r(*mp,-28180));
		*mp=sof[k];
	}
}

// Autocorrelation(5.2.4)
void autocorrelation(int16_t* s,int32_t* L_ACF) {
	int16_t smax=0,temp,scalauto;
	int32_t L_temp;
	int k,i;
	// Search for the maximum
	for(k=0;k<FRAME_LENGTH;k++) {
		temp=fxp_abs(s[k]);
		if(temp>smax) smax=temp;
	}
	// Computation of the scaling factor
	if(smax==0) scalauto=0;
	else scalauto=fxp_sub(4,fxp_L_norm((int32_t)(smax<<16)));
	// Scaling of the array
	if(scalauto>0) {
		temp=16384>>(scalauto-1);
		for(k=0;k<FRAME_LENGTH;k++)
			s[k]=fxp_mult_r(s[k],temp);
	}
	// Compute the autocorrelation
	for(k=0;k<9;k++) {
		L_ACF[k]=0;
		for(i=k;k<FRAME_LENGTH;k++) {
			L_temp=fxp_L_mult(s[i],s[i-k]);
			L_ACF[k]=fxp_L_add(L_ACF[k],L_temp);
		}
	}
	// Rescaling the array
	if(scalauto>0)
		for(k=0;k<FRAME_LENGTH;k++)
			s[k]<<=scalauto;
}

// Computation of the reflection coefficients(5.2.5)
void reflection_coeffs(int32_t *L_ACF,int16_t *r) {
	int i,k,m,n;
	int16_t temp;
	int16_t ACF[9],K[9],P[9];
	// Schur recusrions with 16 bit arithmetic
	if(L_ACF[0]==0) {
		for(i=1;i<=8;i++)
			r[i]=0;
		return;
	}
	temp=fxp_L_norm(L_ACF[0]);
	for(k=0;k<9;k++)
		ACF[k]=(int16_t)((L_ACF[k]<<temp)>>16);
	// Initialize P and K arrays for recursion
	for(i=1;i<=7;i++)
		K[9-i]=ACF[i];
	for(i=0;i<9;i++)
		P[i]=ACF[i];
	// Compute reflection coefficients
	for(n=1;n<9;n++) {
		if(P[0]<fxp_abs(P[1])) {
			for(i=n;i<9;i++)
				r[i]=0;
			return;
		}
		r[n]=fxp_div(fxp_abs(P[1]),P[0]);
		if(P[1]>0) r[n]=fxp_sub(0,r[n]);
		if(n==8) return;
		// Schur recursion
		P[0]=fxp_add(P[0],fxp_mult_r(P[1],r[n]));
		for(m=1;m<=8-n;m++) {
			P[m]=fxp_add(P[m+1],fxp_mult_r(K[9-m],r[n]));
			K[9-m]=fxp_add(K[9-m],fxp_mult_r(P[m+1],r[n]));
		}
	}
}

// Transform the reflection coefficients to Log-Area Ratios(5.2.6)
// The following scaling has been used
/* r[..] = integer( real_r[..]*32768. ); -1. <= real_r <1. */
/* */
/* LAR[..] = integer( real_LAR[..]*16384. ); */
/* */
/* with -1.625 <= real_LAR <= 1.625 */
void refl_to_lar(int16_t* r, int16_t* LAR) {
	int16_t temp;
	int i;
	for(i=1;i<9;i++) {
		temp=fxp_abs(r[i]);
		if(temp<22118) 
			temp>>=1;
		else if(temp<31130)
			temp=fxp_sub(temp,11059);
		else
			temp=fxp_sub(temp,26112)<<2;
		LAR[i]=temp;
		if(r[i]<0) LAR[i]=fxp_sub(0,LAR[i]);
	}
}

// Quantization and coding of the Log-Area Ratios(5.2.7)
void lar_to_larc(int16_t* LAR,int16_t* LARc) {
	int i;
	int16_t temp;
	for(i=1;i<9;i++) {
		temp=fxp_mult(table_A[i],LAR[i]);
		temp=fxp_add(temp,table_B[i]);
		temp=fxp_add(temp,256);
		LARc[i]=temp>>9;
		// Check if LARc[i] lies between MIN and MAX
		if(LARc[i]>table_MAC[i]) LARc[i]=table_MAC[i];
		if(LARc[i]>table_MIC[i]) LARc[i]=table_MIC[i];
		// Used to make all the LARc[i] positive
		LARc[i]=fxp_sub(LARc[i],table_MIC[i]);
	}	
}

// Calculation of the LTP parameters(5.2.11)
void ltp_params(int16_t* d,int16_t* dp,int* bc,int* Nc,int k) {
	static int32_t wt[40];
	int32_t L_max,L_result,L_temp,L_power;
	int16_t temp,dmax=0,R,S;
	int i;
	int k_start,k_end;
	int scal;
	int lambda;
	k_start = k * 40;
	k_end = k_start + 39;
	// Search the optimal scaling of d
	for(i=k_start;i<=k_end;i++) {
		temp=fxp_abs(d[i]);
		if(temp>dmax) dmax = temp;
	}
	temp=0;
	if(dmax==0) scal=0;
	else temp = fxp_L_norm(dmax<<16);
	if(temp>6) scal=0;
	else scal = fxp_sub(6,temp);
	// Initialization of a working array wt[0..39]
	for(i=0;i<40;i++) wt[i] = d[i] >> scal;
	// Search for the maximum cross-correlation and coding of the LTP lag
	L_max=0;
	*Nc=40; // index for the maximum cross-correlation
	for(lambda=40;lambda<=120;lambda++) {
		L_result=0;
		for(i=0;i<40;i++) {
			L_temp = fxp_L_mult(wt[i],dp[119+i-lambda]);
			L_result = fxp_L_add(L_temp,L_result);
		}
		if(L_result>L_max) {
			*Nc = lambda;
			L_max = L_result;
		}
	}
	// Rescaling of L_max
	L_max = L_max >> (fxp_sub(6,scal));
	// Initialization of a working array wt[0..39]
	for(i=0;i<40;i++) wt[i] = dp[119+i-(*Nc)] >> 3; // ?????????? i-Nc < 0 !!!!!!!!!!!!!!!
	// Compute the power of the reconstructed short term residual signal dp[..].
	L_power = 0;
	for(i=0;i<40;i++) {
		L_temp = fxp_L_mult(wt[i],wt[i]);
		L_power = fxp_L_add(L_temp,L_power);
	}
	// Normalization of L_max and L_power.
	if(L_max<=0) { *bc=0; return; }
	if(L_max>=L_power) { *bc=3;return; }
	temp=fxp_L_norm(L_power);
	R = (L_max << temp) >> 16;
	S = (L_power << temp) >> 16;
	// Coding of the LTP gain.
	// Table 5.3a shall be used to obtain the level DLB[i] for the quantization of the LTP gain b to get the coded version bc.
	for(i=0;i<=3;i++) 
		if(R <= fxp_mult(S, table_DLB[i])) { // TODO : declare table_DLB
			*bc=i;
			return;
		}
	*bc=3;	
//
// Rescaling of L_max.
// L_max = L_max >> ( sub( 6, scal ) );
// Initialization of a working array wt[0..39].
// |== FOR k = 0 to 39:
// | wt[k] = dp[k-Nc] >> 3;
// |== NEXT k:
// Compute the power of the reconstructed short term residual signal dp[..].
// L_power = 0;
// |== FOR k =0 to 39:
// | L_temp = L_mult( wt[k], wt[k] );
// | L_power = L_add( L_temp, L_power );
// |== NEXT k:
// Normalization of L_max and L_power.
// IF ( L_max <= 0 ) THEN
// | bc = 0;
// | EXIT; /cont. with 5.2.12/
//
// IF ( L_max >= L_power ) THEN
// | bc = 3;
// | EXIT; /cont. with 5.2.12/
// temp = norm( L_power );
// R = ( L_max << temp ) >> 16;
// S = ( L_power << temp ) >> 16;
// Coding of the LTP gain.
// Table 5.3a shall be used to obtain the level DLB[i] for the quantization of the LTP gain b to get the coded version bc.
// |== FOR bc = 0 to 2:
// | IF (R <= mult(S, DLB[bc])) THEN EXIT; /cont. with
// 5.2.12/
// |== NEXT bc;
// bc = 3;
// Initial value: dp[-120..-1]=0;

}

// Long term analysis filtering (5.2.12)
void lt_filter(int bc, int Nc, int16_t* d,int16_t* dp, int16_t* dpp,int16_t* e,int k) {
/*
Decoding of the coded LTP gain.
bp = QLB[bc];
Calculating the array e[0..39] and the array dpp[0..39].
|== FOR k = 0 to 39:
| dpp[k] = mult_r( bp, dp[k-Nc] );
| e[k] = sub( d[k], dpp[k] );
|== NEXT k:
RPE encoding clause
*/
	int i;
	int bp = table_QLB[bc];//TODO declare table_QLB
	int k_start,k_end;

	k_start = k * 40;
	k_end = k_start + 39;
	for(i=0;i<40;i++) {
		dpp[k_start+i] = fxp_mult_r(bp,dp[119+i-Nc]);
		e[k_start+i] = fxp_sub(d[k_start+i],dpp[k_start+i]);
	}
}

// Weighting filter (5.2.13)
void w_filter(int16_t*e ,int16_t* x,int k) {
/*
The coefficients of the weighting filter are stored in a table (see table 5.4). The following scaling is used:
 H[0..10] = integer( real_H[0..10]*8192 );
Initialization of a temporary working array wt[0..49].
|== FOR k= 0 to 4:
| wt[k] = 0;
|== NEXT k:
|== FOR k = 5 to 44:
| wt[k] = e[k-5];
|== NEXT k:
|== FOR k= 45 to 49:
| wt[k] = 0;
|== NEXT k:
Compute the signal x[0..39].
|== FOR k= 0 to 39:
| L_result = 8192; /rounding of the output
of the filter/
|==== FOR i = 0 to 10:
| L_temp = L_mult( wt[k+i], H[i] );
| L_result = L_add( L_result, L_temp );
|==== NEXT i:
| L_result = L_add(L_result,L_result); /scaling (x2)/
| L_result = L_add(L_result,L_result); /scaling (x4)/
| x[k] = L_result >> 16;
|== NEXT k:
*/
	int i,j;
	static int16_t wt[40];
	int32_t L_result,L_temp;
	int k_start,k_end;

	k_start = k * 40;
	k_end = k_start + 39;
	//Initialization of a temporary working array wt[0..49].
	for(i=0;i<5;i++) wt[i]=0;
	for(i=5;i<45;i++) wt[i]=e[k_start+i-5];
	for(i=45;i<50;i++) wt[i]=0;
	// Compute the signal x[0..39].
	for(i=0;i<40;i++) {
		L_result = 8192;
		for(j=0;j<11;j++) {
			L_temp = fxp_L_mult(wt[i+j],table_H[j]);
			L_result = fxp_L_add(L_result,L_temp);
		}
		L_result = fxp_L_add(L_result,L_result);
		L_result = fxp_L_add(L_result,L_result);
		x[i] = L_result >> 16;
	}

}

// RPE grid selection(5.2.14)
void grid_selection(int16_t* x, int* Mc,int k) {
	int m,i;
	int32_t L_result,EM=0,L_temp;
	int16_t temp1;
	*Mc=0;
	for(m=0;m<4;m++) {
		L_result = 0;
		for(i=0;i<13;i++) {
			temp1=x[m+(3*i)] >> 2;
			L_temp=fxp_L_mult(temp1,temp1);
			L_result=fxp_L_add(L_temp,L_result);
		}
		if(L_result > EM) {
			*Mc=m;
			EM=L_result;
		}
	}
/*
The signal x[0..39] is used to select the RPE grid which is represented byMc.
EM =0;
Mc = 0;
|== FOR m = 0 to 3:
| L_result = 0;
|==== FOR i = 0 to 12:
| temp1 = x[m+(3*i)] >> 2;
| L_temp = L_mult( temp1, temp1 );
| L_result = L_add( L_temp, L_result );
|==== NEXT i:
| IF ( L_result > EM) THEN
| | Mc = m;
| | EM = L_result;
|== NEXT m:


*/
}
void down_sampling_by3(int16_t*x,int Mc,int16_t*xM,int k) {
	int i;
	for(i=0;i<13;i++) xM[i] = x[Mc + (3*i)];
/*
Down-sampling by a factor 3 to get the selected xM[0..12] RPE sequence.
|== FOR i = 0 to 12:
| xM[i] = x[Mc +(3*i)];
|== NEXT i:
*/
}

// APCM quantization of the selected RPE sequence(5.2.15)
void apcm_q_of_rpe_seq(int16_t*xM, int16_t* xmaxc, int* mant, int *exp,int16_t* xMc,int k) {
	// Find the maximum absolute value xmax of xM[0..12].
	int i,itest;
	int16_t temp,xmax=0,temp1,temp2;
	for(i=0;i<13;i++) {
		temp=fxp_abs(xM[i]);
		if(temp>xmax) xmax=temp;
	}
	//Quantizing and coding of xmax to get xmaxc.
	*exp=0;
	temp=xmax >> 9;
	itest = 0;
	for(i=0;i<6;i++) {
		if(temp<=0) itest=1;
		temp >>=1;
		if(itest==0) *exp=fxp_add(*exp,1);
	}
	temp = fxp_add(*exp,5);
	*xmaxc=fxp_add(xmax>>temp,*exp<<3);
	//Quantizing and coding of the xM[0..12] RPE sequence to get the xMc[0..12].
	*exp=0;
	if ( *xmaxc > 15 ) *exp = fxp_sub( ( *xmaxc >> 3 ), 1 ) ;
	*mant = fxp_sub( *xmaxc , ( *exp << 3 ) );
	// Normalize mantissa 0 <= mant <= 7.
	if ( *mant == 0 ) {*exp = -4;
	*mant = 15;
	}
	else  itest = 0;
	for(i=0;i<=2;i++) {
		if ( *mant > 7 ) itest = 1;
		if (itest == 0) *mant = fxp_add((*mant << 1),1);
		if ( itest == 0 ) *exp = fxp_sub( *exp, 1 );
	}
	*mant = fxp_sub( *mant, 8 );
	// Direct computation of xMc[0..12] using table 5.5.
	temp1= fxp_sub( 6, *exp ); //normalization by the exponent
	temp2 = table_NRFAC[*mant]; // TODO see table 5.5 (inverse mantissa)
	for(i=0;i<13;i++) {
		temp = xM[i] << temp1;
		temp = fxp_mult( temp , temp2 );
		xMc[i] = fxp_add( ( temp >> 12 ), 4 );
	}

/*
Find the maximum absolute value xmax of xM[0..12].
xmax = 0;
|== FOR i = 0 to 12:
| temp = abs( xM[i] ) ;
| IF ( temp > xmax ) THEN xmax = temp;
|== NEXT i:
Quantizing and coding of xmax to get xmaxc.
exp = 0;
temp = xmax >> 9;
itest = 0;
|== FOR i = 0 to 5:
| IF ( temp <= 0 ) THEN itest = 1;
| temp = temp >> 1;
| IF ( itest == 0 ) THEN exp = add( exp, 1 ) ;
|== NEXT i:
temp = add( exp, 5 ) ;
xmaxc = add( ( xmax >> temp ), ( exp << 3 ) ) ;
Quantizing and coding of the xM[0..12] RPE sequence to get the xMc[0..12].
This computation uses the fact that the decoded version of xmaxc can be calculated by using the exponent and the
mantissa part of xmaxc (logarithmic table).
So, this method avoids any division and uses only a scaling of the RPE samples by a function of the exponent. A direct
multiplication by the inverse of the mantissa (NRFAC[0..7] found in table 5.5) gives the 3 bit coded version xMc[0..12]
of the RPE samples.
Compute exponent and mantissa of the decoded version of xmaxc.
exp = 0 ;
IF ( xmaxc > 15 ) THEN exp = sub( ( xmaxc >> 3 ), 1 ) ;
mant = sub( xmaxc , ( exp << 3 ) );
Normalize mantissa 0 <= mant <= 7.
IF ( mant == 0 ) THEN | exp = -4;
| mant = 15;
ELSE | itest = 0;
|== FOR i = 0 to 2:
| IF ( mant > 7 ) THEN itest = 1;
| IF (itest == 0) THEN mant = add((mant << 1),1);
| IF ( itest == 0 ) THEN exp = sub( exp, 1 );
|== NEXT i:
mant = sub( mant, 8 );
Direct computation of xMc[0..12] using table 5.5.
temp1= sub( 6, exp ); /normalization by the exponent/
temp2 = NRFAC[mant]; /see table 5.5 (inverse mantissa)/
|== FOR i = 0 to 12:
| temp = xM[i] << temp1;
| temp = mult( temp , temp2 );
| xMc[i] = add( ( temp >> 12 ), 4 ); /See note below/
|== NEXT I:
NOTE: This equation is used to make all the xMc[i] positive.
Keep in memory exp and mant for the following inverse APCM quantizer.


*/
}

// Update of the reconstructed short term residual signal [-120..-1](5.2.18)
void update_reconstructed_st_residual_signal(int16_t* ep,int16_t* dpp,int16_t* dp,int k) {
	int i;
	for(i=0;i<80;i++)
		dp[119-i] = dp[79-i];
		
	for(i=0;i<40;i++)
		dp[39-i] = fxp_add( ep[i], dpp[i] );
/*
This procedure adds the reconstructed long term residual signal ep[0..39] to the estimated signal dpp[0..39] from the
long term analysis filter to compute the reconstructed short term residual signal dp[-40..-1]; also the reconstructed short
term residual array dp[-120..-41] is updated.
|== FOR k = 0 to 79:
| dp[-120+k] = dp[-80+k];
|== NEXT k:
|== FOR k = 0 to 39:
| dp[-40+k] = add( ep[k], dpp[k] );
|== NEXT k:
Keep the array dp[-120..-1] in memory for the next sub-segment.
Initial value: dp[-120..-1]=0;
 */
}

void rpeltp_encoder(
		int16_t* p_frame,
		// Output
		// For each primary frame
		int16_t* LARc,
		// For each secondary frame
		int16_t* LTP_P1,
		int16_t* LTP_b1,
		int16_t* RPE_M1,
		int16_t* RPE_xmaxc1,
		int16_t* RPE_xMc1,

		int16_t* LTP_P2,
		int16_t* LTP_b2,
		int16_t* RPE_M2,
		int16_t* RPE_xmaxc2,
		int16_t* RPE_xMc2,

		int16_t* LTP_P3,
		int16_t* LTP_b3,
		int16_t* RPE_M3,
		int16_t* RPE_xmaxc3,
		int16_t* RPE_xMc3,

		int16_t* LTP_P4,
		int16_t* LTP_b4,
		int16_t* RPE_M4,
		int16_t* RPE_xmaxc4,
		int16_t* RPE_xMc4
		)
{
	// State variables
	static int16_t p_frame_buffer1[160];// Hold result of offset compensation
	static int16_t p_frame_buffer2[160];// Hold result of pre-emphasis
	static int32_t L_ACF[9]; // Hold autocorrelation values
	static int16_t r_coeffs[8]; // Hold reflection coefficients
	static int16_t LAR[8]; // Hold uncoded LAR
	static int16_t LARpp[8];
	static int16_t prev_LARpp[8];
	static int16_t LARp[8];
	static int16_t rp[8];
	static int16_t u[8];
	static int16_t d[160];
	static int16_t dp[160];
	static int16_t dpp[160];
	static int16_t e[160];
	static int16_t ep[160];
	static int16_t x[40];
	static int16_t xM[13];
	static int16_t xMp[13];
	static int16_t z1;
	static int32_t L_z2;
	static int16_t mp;

	// Other variables
	int16_t* LTP_P[] = {LTP_P1,LTP_P2,LTP_P3,LTP_P4};
	int16_t* LTP_b[] = {LTP_b1,LTP_b2,LTP_b3,LTP_b4};
	int16_t* RPE_M[] = {RPE_M1,RPE_M2,RPE_M3,RPE_M4};
	int16_t* RPE_xmaxc[] = {RPE_xmaxc1,RPE_xmaxc2,RPE_xmaxc3,RPE_xmaxc4};
	int16_t* RPE_xMc[] = {RPE_xMc1,RPE_xMc2,RPE_xMc3,RPE_xMc4};
	int k=0;
	int mant,exp;


	// Input downscale
	input_downscale(p_frame);
	// Offset compensation(5.2.2)
	// sof[k]=so[k]-so[k-1]+alpha*sof[k-1]
	// alpha=32735*2^-5
	offset_compensation(p_frame,p_frame_buffer1,&z1,&L_z2);
	// Pre-emphasis(5.2.3)
	pre_emphasis(p_frame_buffer1,p_frame_buffer2,&mp);
	// Autocorrelation(5.2.4)
	autocorrelation(p_frame_buffer2,L_ACF);
	// Computation of the reflection coefficients(5.2.5)
	reflection_coeffs(L_ACF,r_coeffs);
	// Transform the reflection coefficients to Log-Area Ratios(5.2.6)
	refl_to_lar(r_coeffs, LAR);
	// Quantization and coding of the Log-Area Ratios(5.2.7)
	lar_to_larc(LAR,LARc);
	larc_to_larpp(LARc,LARpp);
	// larc_to_rp(LARc,rp);
	// Decoding of the coded Log-Area Ratios(5.2.8)
	//larc_to_larpp(LARc,LARpp);
	// Computation of the quantized reflection coefficients(5.2.9)
	// Interpolation of the LARpp to get the LARpi(5.2.9.1)
	// Computation of the rp[1..8] from the interpolated LARp[1..8] (5.2.9.2)
	// ?
	// Short term filtering
	//st_filter(p_frame_buffer2, d, rp, u, prev_LARpp, LARpp, LARp);
	st_filter(p_frame_buffer2,d,LARpp,prev_LARpp,u,rp,LARp);
	// For each secondary frame
	for(k=0;k<4;k++) {
		// Calculation of the LTP parameters(5.2.11)
		ltp_params(d,dp,(int*)LTP_b[k],(int*)LTP_P[k],k);
		
		// Long term analysis filtering (5.2.12)
		lt_filter(*LTP_b[k], *LTP_P[k], d, dp, dpp,e, k);
		
		// Weighting filter (5.2.13)
		w_filter(e, x, k);
		
		// RPE grid selection(5.2.14)
		grid_selection(x, (int*)RPE_M[k], k);
		down_sampling_by3(x,*RPE_M[k],xM,k);
		
		// APCM quantization of the selected RPE sequence(5.2.15)
		apcm_q_of_rpe_seq(xM,RPE_xmaxc[k], &mant, &exp, RPE_xMc[k],k);
		
		// APCM inverse quatization(5.2.16)
		apcm_iq_of_rpe_seq(RPE_xMc[k], mant, exp,xMp,k);
		
		// RPE grid positioning(5.2.17)
		rpe_grid_positioning(xMp, *RPE_M[k],ep,k);
		
		// Update of the reconstructed short term residual signal [-120..-1](5.2.18)
		update_reconstructed_st_residual_signal(ep,dpp,dp,k);
	}
	// TODO : Build the homing frame
	
}



void pack_decoder_homing_frame(
				// Input
				// For each primary frame
				int16_t* LARc,
				// For each secondary frame
				int16_t* LTP_P1,
				int16_t* LTP_b1,
				int16_t* RPE_M1,
				int16_t* RPE_xmaxc1,
				int16_t* RPE_xMc1,

				int16_t* LTP_P2,
				int16_t* LTP_b2,
				int16_t* RPE_M2,
				int16_t* RPE_xmaxc2,
				int16_t* RPE_xMc2,

				int16_t* LTP_P3,
				int16_t* LTP_b3,
				int16_t* RPE_M3,
				int16_t* RPE_xmaxc3,
				int16_t* RPE_xMc3,

				int16_t* LTP_P4,
				int16_t* LTP_b4,
				int16_t* RPE_M4,
				int16_t* RPE_xmaxc4,
				int16_t* RPE_xMc4,
				// Output - Packed voice frame
				uint8_t* dhf
		) {
	// Pack the LAR coefficients
	// TODO : check the shifts
	// byte 0  : 6 msb LARc[0], 2 lsb LARc[1]
	dhf[0] = (LARc[0]<<2) + get_first_bits(LARc[1],6,2);
	// byte 1  : 4 msb LARc[1], 4 lsb LARc[2]
	dhf[1] = (get_last_bits(LARc[1],4)<<4) + get_first_bits(LARc[2],5,4);
	// byte 2  : 1 msb LARc[2], 5 lsb LARc[3] , 2 lsb LARc[4]
	dhf[2] = (get_last_bits(LARc[2],1)<<7) + (LARc[3]) + get_first_bits(LARc[4],4,2);
	// byte 3  : 2 msb LARc[4], 4 lsb LARc[5], 2 lsb LARc[6]
	dhf[3] = (get_last_bits(LARc[4],2)<<6) + (LARc[5]<<2) + get_first_bits(LARc[6],3,2);
	// byte 4  : 1 msb LARc[6], 3 lsb LARc[7], 4 lsb N1
	dhf[4] = (get_last_bits(LARc[6],1)<<7) + (LARc[7]<<6) + get_first_bits(*LTP_P1,7,4);

	// byte 5  : 3 msb N1, 2 lsb b1, 2 lsb M1, 1 lsb Xmax1
	dhf[5] = (get_last_bits(*LTP_P1,3)<<5) + (*LTP_b1<<3) + (*RPE_M1<<1) + get_first_bits(*RPE_xmaxc1,6,1);
	// byte 6  : 5 msb Xmax1, 3 lsb x1(0)
	dhf[6] = (get_last_bits(*RPE_xmaxc1,5)<<3) + RPE_xMc1[0];
	// byte 7  : 3 msb x1(1), 3 lsb x1(2), 2 lsb x1(3)
	dhf[7] = (RPE_xMc1[1]<<5) + (RPE_xMc1[2]<<3) + get_first_bits(RPE_xMc1[3],3,2);
	// byte 8  : 1 msb x1(3), 3 lsb x1(4), 3 lsb x1(5), 1 lsb x1(6)
	dhf[8] = (get_last_bits(RPE_xMc1[3],1)<<7) + (RPE_xMc1[4]<<3) + get_first_bits(RPE_xMc1[6],3,1);
	// byte 9  : 2 lsb x1(6), 3 lsb x1(7), 3 lsb x1(8)
	dhf[9] = (get_last_bits(RPE_xMc1[6],2)<<6) + (RPE_xMc1[7]<<3) + RPE_xMc1[8];
	// byte 10 : 3 lsb x1(9), 3 lsb x1(10), 2 lsb x1(11)
	dhf[10] = (RPE_xMc1[9]<<5) + (RPE_xMc1[10]<<2) + get_first_bits(RPE_xMc1[11],3,2);
	// byte 11 : 1 lsb x1(11), 3 lsb x1(12), 4 lsb N2
	dhf[11] = (get_last_bits(RPE_xMc1[11],1)<<7) + (RPE_xMc1[12]<<4) + get_first_bits(*LTP_P2,7,4);

	// byte 12 : 3 msb N2, 2 lsb b2, 2 lsb M2, 1 lsb Xmax2
	dhf[12] = (get_last_bits(*LTP_P2,3)<<5) + (*LTP_b2<<3) + (*RPE_M2<<1) + get_first_bits(*RPE_xmaxc2,6,1);
	// byte 13 : 5 msb Xmax1, 3 lsb x2(0)
	dhf[13] = (get_last_bits(*RPE_xmaxc2,5)<<3) + RPE_xMc2[0];
	// byte 14 : 3 msb x2(1), 3 lsb x2(2), 2 lsb x2(3)
	dhf[14] = (RPE_xMc2[1]<<5) + (RPE_xMc2[2]<<3) + get_first_bits(RPE_xMc2[3],3,2);
	// byte 15 : 1 msb x2(3), 3 lsb x2(4), 3 lsb x2(5), 1 lsb x2(6)
	dhf[15] = (get_last_bits(RPE_xMc2[3],1)<<7) + (RPE_xMc2[4]<<3) + get_first_bits(RPE_xMc2[6],3,1);
	// byte 16 : 2 lsb x2(6), 3 lsb x2(7), 3 lsb x2(8)
	dhf[16] = (get_last_bits(RPE_xMc2[6],2)<<6) + (RPE_xMc2[7]<<3) + RPE_xMc2[8];
	// byte 17 : 3 lsb x2(9), 3 lsb x2(10), 2 lsb x2(11)
	dhf[17] = (RPE_xMc2[9]<<5) + (RPE_xMc2[10]<<2) + get_first_bits(RPE_xMc2[11],3,2);
	// byte 18 : 1 lsb x2(11), 3 lsb x2(12), 4 lsb N2
	dhf[18] = (get_last_bits(RPE_xMc2[11],1)<<7) + (RPE_xMc2[12]<<4) + get_first_bits(*LTP_P3,7,4);

	// byte 19 : 3 msb N3, 2 lsb b3, 2 lsb M3, 1 lsb Xmax3
	dhf[19] = (get_last_bits(*LTP_P3,3)<<5) + (*LTP_b3<<3) + (*RPE_M3<<1) + get_first_bits(*RPE_xmaxc3,6,1);
	// byte 20 : 5 msb Xmax3, 3 lsb x3(0)
	dhf[20] = (get_last_bits(*RPE_xmaxc3,5)<<3) + RPE_xMc3[0];
	// byte 21 : 3 msb x3(1), 3 lsb x3(2), 2 lsb x3(3)
	dhf[21] = (RPE_xMc3[1]<<5) + (RPE_xMc3[2]<<3) + get_first_bits(RPE_xMc3[3],3,2);
	// byte 22 : 1 msb x3(3), 3 lsb x3(4), 3 lsb x3(5), 1 lsb x3(6)
	dhf[22] = (get_last_bits(RPE_xMc3[3],1)<<7) + (RPE_xMc3[4]<<3) + get_first_bits(RPE_xMc3[6],3,1);
	// byte 23 : 2 lsb x3(6), 3 lsb x3(7), 3 lsb x3(8)
	dhf[23] = (get_last_bits(RPE_xMc3[6],2)<<6) + (RPE_xMc3[7]<<3) + RPE_xMc3[8];
	// byte 24 : 3 lsb x3(9), 3 lsb x3(10), 2 lsb x3(11)
	dhf[24] = (RPE_xMc3[9]<<5) + (RPE_xMc3[10]<<2) + get_first_bits(RPE_xMc3[11],3,2);
	// byte 25 : 1 lsb x3(11), 3 lsb x3(12), 4 lsb N2
	dhf[25] = (get_last_bits(RPE_xMc3[11],1)<<7) + (RPE_xMc3[12]<<4) + get_first_bits(*LTP_P4,7,4);

	// byte 26 : 3 msb N2, 2 lsb b2, 2 lsb M2, 1 lsb Xmax2
	dhf[26] = (get_last_bits(*LTP_P4,3)<<5) + (*LTP_b4<<3) + (*RPE_M4<<1) + get_first_bits(*RPE_xmaxc4,6,1);
	// byte 27 : 5 msb Xmax1, 3 lsb x4(0)
	dhf[27] = (get_last_bits(*RPE_xmaxc4,5)<<3) + RPE_xMc4[0];
	// byte 28 : 3 msb x4(1), 3 lsb x4(2), 2 lsb x4(3)
	dhf[28] = (RPE_xMc4[1]<<5) + (RPE_xMc4[2]<<3) + get_first_bits(RPE_xMc4[3],3,2);
	// byte 29 : 1 msb x4(3), 3 lsb x4(4), 3 lsb x4(5), 1 lsb x4(6)
	dhf[29] = (get_last_bits(RPE_xMc4[3],1)<<7) + (RPE_xMc4[4]<<3) + get_first_bits(RPE_xMc4[6],3,1);
	// byte 30 : 2 lsb x4(6), 3 lsb x4(7), 3 lsb x4(8)
	dhf[30] = (get_last_bits(RPE_xMc4[6],2)<<6) + (RPE_xMc4[7]<<3) + RPE_xMc4[8];
	// byte 31 : 3 lsb x4(9), 3 lsb x4(10), 2 lsb x4(11)
	dhf[31] = (RPE_xMc4[9]<<5) + (RPE_xMc4[10]<<2) + get_first_bits(RPE_xMc4[11],3,2);
	// byte 32 : 1 lsb x4(11), 3 lsb x4(12), 4 lsb N2
	dhf[32] = (get_last_bits(RPE_xMc4[11],1)<<7) + (RPE_xMc4[12]<<4);

}

